#include "topology.h"

const int n_sigma = 3;        // 拓扑图数目
double switch_interval = 3;   // 切换周期
double weight_switch[3] = {0.4, 0.4, 0.2}; // 间隔权重
double miu = 1.66677;             // 观测器状态反馈系数

// 通信拓扑权重矩阵、拉普拉斯矩阵
Eigen::MatrixXi Wmat[3];
Eigen::Matrix4i Lapl[3];
Eigen::Matrix4d aver_Lap = Eigen::Matrix4d::Zero();

// 初始化权重矩阵、拉普拉斯矩阵
// void init_topo(void)
// {
//     // 拉普拉斯
//     Eigen::MatrixXi temp;
//     Eigen::VectorXi degree_temp;
//     for(int i = 0; i < n_sigma; i++)
//     {
//         degree_temp = Wmat[i].rowwise().sum();
//         temp = -Wmat[i] + degree_temp.asDiagonal().toDenseMatrix();
//         Lapl[i] = temp.block<4,4>(1,1);
//         // cout << i << "\n" << Lapl[i] << "\n\n";
//         aver_Lap += Lapl[i].cast<double>() * weight_switch[i];
//     }

//     // 平均拉普拉斯矩阵
//     // cout << aver_Lap << "\n\n";
//     miu = 1.0 / aver_Lap.eigenvalues().real().minCoeff(); 
//     // cout << miu << endl;
// }

void init_topo(void)
{
    // 权重
    Wmat[0] = (Eigen::MatrixXi(5,5) << 
        0, 0, 0, 0, 0,
        1, 0, 0, 0, 0,
        0, 1, 0, 0, 0,
        0, 1, 0, 0, 0,
        0, 0, 1, 0, 0
    ).finished();

    Wmat[1] = (Eigen::MatrixXi(5,5) << 
        0, 0, 0, 0, 0,
        1, 0, 0, 0, 0,
        0, 1, 0, 0, 0,
        0, 0, 0, 0, 1,
        0, 0, 1, 0, 0
    ).finished();

    Wmat[2] = (Eigen::MatrixXi(5,5) << 
        0, 0, 0, 0, 0,
        0, 0, 0, 0, 0,
        0, 0, 0, 0, 0,
        0, 0, 0, 0, 0,
        0, 0, 0, 0, 0
    ).finished();

    // 拉普拉斯
    Eigen::MatrixXi temp;
    Eigen::VectorXi degree_temp;
    for(int i = 0; i < n_sigma; i++)
    {
        degree_temp = Wmat[i].rowwise().sum();
        temp = -Wmat[i] + degree_temp.asDiagonal().toDenseMatrix();
        Lapl[i] = temp.block<4,4>(1,1);
        // cout << i << "\n" << Lapl[i] << "\n\n";
        aver_Lap += Lapl[i].cast<double>() * weight_switch[i];
    }

    // 平均拉普拉斯矩阵
    // cout << aver_Lap << "\n\n";
    miu = 1.0 / aver_Lap.eigenvalues().real().minCoeff(); 
    // cout << miu << endl;
}
